%% Newton-type method with BFGS update
% clear all;
close all;
clc;

% Defining Iteration constants
n_it = 500;
gamma = 0.1;
beta = 0.8;
alpha=100;
sigma_min = 0.1;
infeas = [];
tol = 1E-2;
% Defining physical constants
Restlength = 0.8;
D = 600;
m = 10;
g = 9.81;
% Initial value: tensor is a 3d matrix which contains a X, Y and Z
% coordinate for every point in the grid.
N=3;
tensor = zeros(N,N,3);
for i=1:N,
    % X and Y coordinates are evenly spaced
    tensor(i,:,1)=linspace(-N/2+0.01,N/2-0.01,N);
    tensor(:,i,2)=linspace(-N/2+0.01,N/2-0.01,N);
end
% The starting height (Z coordinate) for every point is 1
tensor(:,:,3)=1;
% x0
x=tensor2array(tensor);


figure()
% Iteration header
fprintf('It.\t\t||∇L||\t||g||\t||Δx||\tt\n');

% Linearizing objective
[f_val J] = finite_difference(x, @cloth,Restlength,D,m,g);
y=norm(J);
% Linearizing constraint
[g_val Jg ] = finite_difference(x, @cloth_eqconstr,Restlength,D,m,g);
%B0 &lambda0 & lag0
B=eye(length(x));
lag_grad = J';
f_grad= J';
% This is needed to make the QP solver silent
opt = optimset('Display', 'off', 'LargeScale', 'off');
for k = 1 : n_it
   % Building up QP
   c = f_grad;
   
   % This is needed to make the QP solver silent
   opt = optimset('Display', 'off', 'LargeScale', 'off', 'TolX', 1E-20);
   % We solve the QP and obtain search direction "dx" and dual variables
   % "lambda"
   [dx fval flag output lambda] = quadprog((B + B') / 2, c, [], [], Jg, -g_val, [], [], [], opt);
   % We calculate the "sigma" of the merit function
   sigma = norm(lambda.eqlin, inf);
   lambda = lambda.eqlin;
   % Evaluate merit function in the actual iterate
   T_act = f_val + sigma * norm(g_val, 1);
   % Calculate the directional derivative
   T_dir = f_grad' * dx - sigma * norm(g_val, 1);
   % Default value for line-search
   t = 1.0;
   % Preparing the first candidate
   x_new = x + t * dx;
   f_new = cloth(x_new,Restlength,D,m,g);
   g_new = cloth_eqconstr(x_new,Restlength,D,m,g);
   % Evaluate merit function in the actual iterate
   T_new = f_new + sigma * norm(g_new, 1);
   % Checking Armijo's condition (on the merit function)
   while (T_new > T_act + gamma * t * T_dir)
      t = beta * t;
      x_new = x + t * dx;
      f_new = cloth(x_new,Restlength,D,m,g);
      g_new = cloth_eqconstr(x_new,Restlength,D,m,g);
      T_new = f_new + sigma * norm(g_new, inf) ;
   end
 
   % By now the candidate is accepted by Armijo's condition
   [f_val f_grad] = finite_difference(x_new, @cloth,Restlength,D,m,g);
   f_grad = f_grad';
   [g_val Jg ] = finite_difference(x_new, @cloth_eqconstr,Restlength,D,m,g);
   lag_grad_new = f_grad + Jg' * lambda;

   % BFGS update
   s = x_new - x;
   y = lag_grad_new - lag_grad;
   % Calculating theta
   if (y' * s < 0.2 * s' * B * s)
      theta = 0.8 * s' * B * s / (s' * B * s - s' * y);
   else
      theta = 0;
   end
   yhat = y + theta * (B * s - y);
    B = B - B * s * s' * B / (s' * B * s) + yhat * yhat' / (s' * yhat);
     
   
   lag_grad = lag_grad_new;
   x = x_new;
      
   fprintf('%d\t %f\t %f\t %f\t %f\n', k, norm(lag_grad), norm(g_val), norm(dx), t)
   % Checking stopping criterion
   if norm([lag_grad; g_val]) < tol
      fprintf('Convergence achieved\n')
      break
   end
   infeas = [infeas; norm([lag_grad; g_val])];
   % Plotting
   plotcloth(x,Restlength);
   drawnow
    
end
figure;
plot(y);